#ifndef __GREYSCALER_H
#define __GREYSCALER_H

#include <cv.h>
#include <gvars3/instances.h>

class Greyscaler
{
	static const int LOOKUP_SIZE=1000;
	int anLookup[LOOKUP_SIZE+1];
	bool init;
  GVars3::gvar3<double> mgvdWeightGamma;
  GVars3::gvar3<int> mgvnWeightRed;
  GVars3::gvar3<int> mgvnWeightGreen;
  GVars3::gvar3<int> mgvnWeightBlue;
	int mnWeightTotal, mnWeightRed, mnWeightGreen, mnWeightBlue;
  double mdWeightGamma;
public:
	Greyscaler()
	{
    GVars3::GV3::Register(mgvdWeightGamma, "Greyscaler.WeightGamma", 1.0, GVars3::SILENT);
    GVars3::GV3::Register(mgvnWeightRed, "Greyscaler.WeightRed", 250, GVars3::SILENT);
    GVars3::GV3::Register(mgvnWeightGreen, "Greyscaler.WeightGreen", 150, GVars3::SILENT);
    GVars3::GV3::Register(mgvnWeightBlue, "Greyscaler.WeightBlue", 10, GVars3::SILENT);
    mdWeightGamma = *mgvdWeightGamma;
    mnWeightRed = *mgvnWeightRed;
    mnWeightGreen = *mgvnWeightGreen;
    mnWeightBlue = *mgvnWeightBlue;
    mnWeightTotal = mnWeightRed + mnWeightGreen + mnWeightBlue;

		for(int i = 0; i <= LOOKUP_SIZE; i++)
		{
			double dValIn01 = (double)i/(double)LOOKUP_SIZE;
			if(mdWeightGamma != 1)
				dValIn01 = pow(dValIn01, mdWeightGamma);
			anLookup[i] = (int)(255.0*dValIn01);
			if(i > 0 && anLookup[i] > anLookup[i-1]+1)
				std::cout << "Warning: Losing precision in RGB to greyscale conversion." << std::endl;
		}

		if(anLookup[0] != 0 || anLookup[LOOKUP_SIZE] != 255)
    {
      std::cout << "Warning: Greyscale initialisation failed, reverting to OpenCV default." << std::endl;
      init = false;
    }
    else
      init = true;
	}

  void BGR2Grey(const IplImage *pBGR, IplImage *pGrey) const
	{
    const double wr=0.2989959716796875, wg=0.587005615234375, wb=0.1139984130859375;

	  uchar * pSrcRow = (uchar *)pBGR->imageData;
	  uchar * pDstRow = (uchar *)pGrey->imageData;
	  for(int y=pBGR->height; y>0; y--, pSrcRow += pBGR->widthStep,  pDstRow += pGrey->widthStep)
	  {
		  uchar * pSrc = pSrcRow;
		  uchar * pDst = pDstRow;
		  for(int x=pBGR->width; x>0; x--, pSrc += pBGR->nChannels, pDst++)
		  {
			  const int b = pSrc[2]; //incorrect ordering seems to give better results (essentially swap weighting of b & r)
			  const int g = pSrc[1];
			  const int r = pSrc[0];
			  *pDst = static_cast<uchar>(wb*b+wg*g+wr*r);
		  }
	  }

    /*if(init)
    {
		  uchar * pSrcRow = (uchar *)pBGR->imageData;
		  uchar * pDstRow = (uchar *)pGrey->imageData;
		  for(int y=pBGR->height; y>0; y--, pSrcRow += pBGR->widthStep,  pDstRow += pGrey->widthStep)
		  {
			  uchar * pSrc = pSrcRow;
			  uchar * pDst = pDstRow;
			  for(int x=pBGR->width; x>0; x--, pSrc += pBGR->nChannels, pDst++)
			  {
				  const int b = pSrc[0];
				  const int g = pSrc[1];
				  const int r = pSrc[2];
				  const int weightedSum = ((mnWeightBlue*b+mnWeightGreen*g+mnWeightRed*r) * LOOKUP_SIZE)/(mnWeightTotal * 255);
				  *pDst = (uchar)anLookup[weightedSum];
			  }
		  }
    }
    else
      cvCvtColor(pBGR, pGrey, CV_RGB2GRAY);*/
	}
};

#endif
